/* Initial beliefs and rules */
turnRate(4).
centerThreshold(7).
/* Initial goals */
!start.

/* Plans */

+!start : true <- .puts("ZZZzzz...").

+!trackMe[source(R)] : helping(R) & foundRobot(Robot,Dir,Deg,Depth)
	<-  +trackingRobot(R);
		.send(R,achieve,tryMove);
		.wait({+moved(Dis)[source(R)]},5000);
		!diagnoseRobot(R).

		
+robot(Robot,Dir,Deg,Depth) : helping(R) & foundRobot(Robot,Dir1,Deg1,Dep1) & math.abs(Dep1-Depth) > 0.1
	<- .puts("Detected movement with #{R}");
		+robotMoved(R);
	    -trackingRobot(R).

+!diagnoseRobot(R) : helping(R) & foundRobot(Robot,Dir1,Deg1,Dep1) & robotMoved(R)
	<- .puts("Found odometry problem with #{R}");
	   .send(R,tell,odometry_fault);
	   -helping(R).
	   
+!diagnoseRobot(R) : helping(R) & foundRobot(Robot,Dir1,Deg1,Dep1) & not robotMoved(R)
	<- .puts("Found motion problem with #{R}");
	   .send(R,tell,motion_fault);
	   -helping(R).

+!helpme[source(R)] : true 
	<- +helping(R);
	   !findRobot(R,0);
	   .send(R,tell,found(R)).

+!findRobot(Robot,Turned) : robot(Robot,Dir,Deg,Depth) & centerThreshold(DT) & Deg < DT
	<- .puts("Found #{Robot}");
	   +foundRobot(Robot,Dir,Deg,Depth).
	
+!findRobot(Robot,Turned) : turnRate(Rate) & centerThreshold(DT)
	<- .puts("Finding #{Robot}, turning #{Rate} degrees, turned #{Turned} so far. Threshold is #{DT}");
		turn(right,Rate);
	   !findRobot(Robot,Turned+Rate).

+robot(Robot,Dir,Deg,Depth) : centerThreshold(DT) & Deg < DT
	<-  -+robot(Robot,Dir,Deg,Depth);
		.puts("#{Robot} detected #{Deg} degrees #{Dir}").

